/*
  LivingSpot.cpp
*/


//#include "WProgram.h"
#include "LivingSpot.h"



//LivingSpot::LivingSpot(int threshold, int posX, int posY, int saccadeRadius, int numSample, int ident) {
//    Init(threshold, posX, posY, saccadeRadius, numSample, ident);
//}

// ------------- Destructor ----------------------------------------
// REM: spots should be instantiated using a dynamic array - and then we can use delete! otherwise things are tricky...
/*
LivingSpot::~LivingSpot() {
	numberSpots--; 	
}
*/

void LivingSpot::Init(int threshold, int posX, int posY, int saccadeRadius, int numSample, int id) {
  	
        updateFixedThreshold(FIXED_THRESHOLD);  // default value
        
        // THRESHOLD MODE: (in fact, we should have three modes: auto, pot and fixed)
        if (threshold==-1) {
         setAutoThreshold(true); 
        } else {
         setAutoThreshold(false); 
         updateFixedThreshold(FIXED_THRESHOLD); 
        } 
        
	// defautl behaviour and "alive" mode:
	alive=false;
	behaviourMode=NEWTON_DYNAMICS; // can be: SIMPLE_BOUNCING, FAST_BOUNCING, NEWTON_DYNAMICS, COUNTOUR_FOLLOWING, SNELL_LAW
	
	// first, set values relative to the spot info class (in the old program), useful for sending data for sound generation:
	identifier=id;//numberOfSpots;
	
         //create spot name from the identifier: "spot1", "spot2", ...
        char aux[3];  
        itoa(identifier, aux, 10);
        // strcpy(spotName, "spot"); // in case we want to have names as "spot1" instead of just "1"
        //strcat(spotName,  aux);
        
        strcpy(spotName, aux); // in case the name is just equal to "0", "1", ....
        
        //Serial.print(spotName);
        
        firstTime=true; // this is used to set the x(t-dt) for verdet integration the first time we update the position.
        firstTimeStartRefraction=true; // this is for the snell law mode
             
        phoenixSpeedMode=false;
        phoenixNormSpeed=25;resetSpeedThreshold=6;
        vx=0; vy=0; 
        initVx=0; initVy=0;
        bounce_collisionDamping=1; // 1 means no damping when collision (in bounce mode)
        bounce_movingDamping=0.995; // 1 means no damping as it moves (in bounce mode only)
        
        ax=0; ay=0; 
	updatePosition(posX, posY); // this will also set the value of touchWall
        placeX=posX; placeY=posY; // the special position is set to the initial position.  
	// rem: the value of collision for each spot is calculated outside the class, but must be initialized here
	collision=false;
	// rem: angles of lines calculated in computeNormalVector
	target_angle=0;// the direction of the recentering vector in radians

	// when in newtonian dynamics:
	bouncingForceMode=true; 
        bouncingSpecularMode=false;
	centralForceMode=false; // default mode is CENTRAL force active
	parallelForceMode=false; // default mode is parallel force inactive
	particleInteractionMode=true;// default mode 
	visForceMode=false; // default mode false
	bouncingForceFactor=50;// needs to be calibrated
	centralForceFactor=0.0003; // this is for a SPRING like force! (not gravitational)
	
        // parallel accelearion: (REM: we could use an accelerometer to control this!!!)
        gx=0; 
        gy=-1;
        // 0.049050;
        // this is: 9.81/0.0002/(1000^2) because the unit is minimum mirror precision step, and the time unit is millisecond
        // (which is about 0.2mm at 1meter distance)/ ms^2 
	
         //interForceFactor=10;//7.5;// for a force on 1/r
        interForceFactor=3000; // for a force on 1/r^2
        // interForceFactor=0.00000002; // for a force that is spring-like
        
	visForceFactor=0.001; // fraction of the speed lost to fraction (unit-less), for calculating the viscosity force

	totalForceX=0; totalForceY=0; // acceleration is calculated form this, divided by the mass of the particle
	interForceX=0; interForceY=0; // the force given by interaction with other particles or objects (gravity+electrostatic)
	
	rangeInteractionLong=saccadeRadius*80; // must be calibrated!! (potentiometer?)
        rangeInteractionShort=saccadeRadius*5; // must be calibrated!! (potentiometer?)
		
	mass=0.05;//pmass;   // for calculating the gravitational force, as well as the acceleration
	charge=1;//pcharge; // for calculating electrostatic force
	
	dt=5; // my initial measures gave dt=5ms
        oldTime=micros();
        slowTime=1.0/15; // (the effective dt will be dt*slowTime)
        
	// collision detection (meaning that another particle is in nearby, inside collisionRadius)
	collisionRadius=saccadeRadius*2; // must be calibrated!
	collision=false;

        // border mode:
        borderDamping=true;
        borderMode=NORMAL_REFLEXION;//RESET_POSITION;// RETRO_REFLEXION;
        showLimits=true;
        showGreenFlash=true;
        
        onContour=false; // start as if there was no contour (activate search)
	searchCounter=0;
        pleaseContinue=0;

	xMax = 3650; xMin = 350;
	yMax = 3650; yMin = 350;
	
	_idAv = 0;
	thresholdFactorFB = 1.3;

	speedContourFollowing = 0.85; // controls the speed of contour following (proportion of radius)
	
      #ifdef _NOHARD
      error_angle = 0;
      #else
	error_angle = -0.54;
	#endif

	binary_smoothing_Mode = true;
	
        float whiteProportion=0; // this is the proportion of samples in the white zone in the saccade
	
	//_saccadePosTab = NULL;
	//_saccadeIntesityTab = NULL;
	//_saccadeBinaryTab = NULL;
	//_transBinaryTab = NULL;
	
	setNumSamples(numSample);
	setRadiusSaccade(saccadeRadius);

        IO.setGreenPower(HIGH);
}

void LivingSpot::update(LivingSpot* spotArray, int numSpots) {
	if (alive==true) { // else do nothing
switch (behaviourMode) {
	case SIMPLE_BOUNCING:
                 //keep laser green off (alexis)
                // if (identifier%2==1)   IO.setGreenPower(LOW); // activate green laser
                //else  IO.setGreenPower(HIGH); // laser off
                
		collision = simpleBouncing(numSpots);
                
		break;
	case FAST_BOUNCING:
		collision = fastBouncing();
		break;
	case NEWTON_DYNAMICS:
                // first, compute inter-particle forces if necessary:
                if (particleInteractionMode) {
                //for (int i=0; i<numSpots; i++) {spotArray[i].interForceX=0; spotArray[i].interForceY=0;};
                 for (int i=0; i<numSpots; i++) {
                	spotArray[i].collision=spotArray[i].computeInteraction(spotArray, numSpots); // this will set forceInteractionX and Y for "this" spot, as well as others in the spotArray list. It also computes the present value of "collision"
                 }
                }
                // then, do Euler or Verdet integration:
		newtonDynamics(numSpots);
		break;
	case COUNTOUR_FOLLOWING:
               // if ((identifier%2==0)||(identifier%2==1))  IO.setGreenPower(LOW); // activate green laser
               //  else IO.setGreenPower(HIGH); // laser off
                // IO.setGreenPower(LOW); // activate green laser
               //  IO.setGreenPower(HIGH); // laser off
                
                if (onContour) onContour=moveAlongContour(numSpots);
                else {
                   
                  if ((_x > xMax - 2*_saccadeRadius)||(_y > yMax - 2*_saccadeRadius)||(_x < xMin + 2*_saccadeRadius)||(_y < yMin + 2*_saccadeRadius)) {
                   moveSpot(random(CENTER_AD_MIRROR_X), random(CENTER_AD_MIRROR_Y));
                  }
                   
                  if (pleaseContinue<3) {
                    pleaseContinue++;
                    if(onContour=moveAlongContour(numSpots)) pleaseContinue=0;
                  } else {
                  searchCounter++;
                  onContour=searchContour(numSpots);
                if (searchCounter>=8) { // REM: 15 is a good value (long searching circle, but too big for Seikai Ichi). When this condition is met, move the spot randomly or recenter
                  moveSpot(CENTER_AD_MIRROR_X+identifier*200, CENTER_AD_MIRROR_Y);//random(MIRRORS_HALF), random(MIRRORS_HALF));
                  searchCounter=0;
                }  
              }
                }
                // reverse direction of motion when touching a wall:
                if (touchWall) speedContourFollowing*=-1;
		break;
	case SNELL_LAW:
	        lightRefraction();

		break;
	default:
		break;
}
  // Common action for all modes:
 if (collision) { 
     if (showGreenFlash) {// deactivated for Seikai Ichi... (better bouncing behaviour). 
                IO.setGreenPower(LOW);
                 computeNormalVector(_x, _y, _saccadeRadius);
                 IO.setGreenPower(HIGH);
     }
     if (phoenixSpeedMode) {
        float normSpeed=sqrt(vx*vx+vy*vy);
        if (normSpeed<resetSpeedThreshold) {
          vx=phoenixNormSpeed*nx; // and we assume that firstTimeSpeedNorm>resetSpeedThreshold
          vy=phoenixNormSpeed*ny; 
        }
   }
 }
 
   if (touchWall) {     
   // show contour:
        if (showLimits) {
           IO.setRedPower_mW(MIN_RedLaserPower);
          drawMaxArea(1);
           IO.setGreenPower(LOW);
          drawMaxArea(1);
           IO.setGreenPower(HIGH);
           IO.setRedPower_mW(MAX_RedLaserPower);
        }
   }
   
  }
}
  
void LivingSpot::setPhoenixSpeed(float _phoenixNormSpeed, float _resetSpeedThreshold) {
  vx=0; vy=0;
  phoenixSpeedMode=true; 
  phoenixNormSpeed=_phoenixNormSpeed;  
  resetSpeedThreshold=_resetSpeedThreshold;
} 

// for calibration of delay mirrors:
void LivingSpot::measureAngle() {
   computeNormalVector(2000, 2000, _saccadeRadius);
   if(computeNormalVector(2000, 2000, _saccadeRadius)) {
   for (int i=0; i<3;i++) {
     moveSpot(2000,2000); delayMicroseconds(400); moveSpot(2000+5*dx, 2000+5*dy); delayMicroseconds(400); 
     moveSpot(2000,2000); delayMicroseconds(400);
   }
   }
}

//---------------Reading and Writing private variables---------------
//---------------      (includes security test)      ----------------


// New (1.3.2010): need to use a method to set the speed when in newtonian dynamics, because I am using the verlet integrator:
void LivingSpot::updateSpeed(float speedX, float speedY){
  vx=speedX; 
  vy=speedY;
 //if (behaviourMode==NEWTON_DYNAMICS) {
 //(in fact, no need to check that because the other modes don't use x1 and y1)
   // rem: I assume a dt equal to 5ms/6 (see newtonDynamics())
   x1=_x-2.0*dt*slowTime*speedX;
   y1=_y-2.0*dt*slowTime*speedY;
 //}
}     

//update the position of the spot to the new position
//and test if the current position is inside the mirrors limit.
// REM: actual motion will be done by the function moveSpot(_x,_y)
int LivingSpot::updatePosition(float posX, float posY){
  int returnedValue = 1;
   _x = posX;
   _y = posY;
  switch(borderMode) {// border mode (int borderMode):
      case NORMAL_REFLEXION:
      // normal reflexion:
if (behaviourMode!=NEWTON_DYNAMICS) {
  if(_x > xMax - _saccadeRadius){
		_x = xMax - _saccadeRadius;
		if (borderDamping) vx = -vx*0.98; else  vx = -vx;
		returnedValue = 0;
	}
	if(_y > yMax - _saccadeRadius){
		_y = yMax - _saccadeRadius;
		if (borderDamping) vy = -vy*0.98; else  vy = -vy;
		returnedValue = 0;
	}
	if(_x < xMin + _saccadeRadius){
		_x = xMin + _saccadeRadius;
		if (borderDamping) vx = -vx*0.98; else  vx = -vx;
		returnedValue = 0;
	}
	if(_y < yMin + _saccadeRadius){
		_y = yMin + _saccadeRadius;
		if (borderDamping) vy = -vy*0.98; else  vy = -vy;
		returnedValue = 0;
	}
} else { // this is in the case of VERLET integration:
  if (posX > xMax - _saccadeRadius) { //||(posX < xMin + _saccadeRadius))
      if (borderDamping) {x1=0.85*(xMax - _saccadeRadius)+0.15*_x; // this will effectively invert the direction (x1=posX would do, but I want some damping...)
      // and we don't change x(t) (i.e. the variable _x)
      } else x1= xMax - _saccadeRadius;
      returnedValue = 0;  
 } else _x=posX;
 
   if (posX < xMin + _saccadeRadius) {
      if (borderDamping) {x1=0.85*(xMin + _saccadeRadius)+0.15*_x; // this will effectively invert the direction (x1=posX would do, but I want some damping...)
      // and we don't change x(t) (i.e. the variable _x)
      } else x1= xMin + _saccadeRadius;
      returnedValue = 0;  
 } else _x=posX;
 
  if (posY > yMax - _saccadeRadius) {
      if (borderDamping) {y1=0.85*( yMax - _saccadeRadius)+0.15*_y; // this will effectively invert the direction (x1=posX would do, but I want some damping...)
        } else y1=( yMax - _saccadeRadius);
      returnedValue = 0;  
 } else _y=posY;
 
   if (posY < yMin + _saccadeRadius) {
      if (borderDamping) {y1=0.85*(yMin + _saccadeRadius)+0.15*_y; // this will effectively invert the direction (x1=posX would do, but I want some damping...)
        } else y1=(yMin + _saccadeRadius);
      returnedValue = 0;  
 } else _y=posY;
 
}
      break;
      case RETRO_REFLEXION: // this is a special reflection in which the speed is just inverted when the border is touched:
      if (behaviourMode!=NEWTON_DYNAMICS) {
   if ((posX > xMax - _saccadeRadius)||(posY > yMax - _saccadeRadius)||(posX < xMin + _saccadeRadius)||(posY < yMin + _saccadeRadius)) {
    if (borderDamping) {
      vx=-vx*0.98; 
      vy=-vy*0.98;
    } else {
      vx=-vx; 
      vy=-vy;
    }
     if (posX > xMax - _saccadeRadius) _x = xMax - _saccadeRadius;
     if (posY > yMax - _saccadeRadius) _y = yMax - _saccadeRadius;
     if (posX < xMin + _saccadeRadius) _x = xMin + _saccadeRadius;
     if (posY < yMin + _saccadeRadius) _y = yMin + _saccadeRadius;
    
    returnedValue = 0;
   }
      } else { // case of VERLET integration:
       if ((posX > xMax - _saccadeRadius)||(posY > yMax - _saccadeRadius)||(posX < xMin + _saccadeRadius)||(posY < yMin + _saccadeRadius)) {
     float auxX=_x; _x=x1; x1=auxX;
     float auxY=_y; _y=y1; y1=auxY;
      returnedValue = 0;
      }
      }
      break;
     case RESET_POSITION: 
      if ((posX > xMax - _saccadeRadius)||(posY > yMax - _saccadeRadius)||(posX < xMin + _saccadeRadius)||(posY < yMin + _saccadeRadius)) {
      _x=placeX; _y=placeY;
      vx=initVx; vy=initVy;
      firstTime=true; // to set x1 and y1 in case of VERLET integration
      firstTimeStartRefraction=true; // this is for the snell law mode
      returnedValue = 0;
      }
   default:
  break; 
  }

if (returnedValue==0) {
  touchWall=true;
} else touchWall=false;

// new: because of more precise verdet integration, we need to set for the FIRST time the x(t-dt):
      if (firstTime) {
          firstTime=false;
          x1=placeX;
          y1=placeY;
          
          // stop green and red laser to avoid a line between last position and new start position:
          //IO.setRedPower_mW(0);//Spi.writeOutR(0);
	  IO.setGreenPower(LOW);
	  moveSpot(_x, _y); // we can use moveSpot (the laser will position itself in the center of the saccade)
	  delayMicroseconds(200);
	}

return returnedValue;
}
  


//update the fixed threshold 
//the value -1.0 change the mode to auto threshold
void LivingSpot::updateFixedThreshold(int newThreshold) {
    fixedThreshold = newThreshold;
}

// change thresholding mode:
void LivingSpot::setAutoThreshold(boolean mode) {
  thresholdingMode=mode;
}



//------------Methods related to the spot behaviour and basical drawing-----------


//update the position and move the spot to the new position
int LivingSpot::moveSpot(float posX, float posY){
	int returnedValue = updatePosition(posX, posY);
	IO.setMirrorsXY_AD(int(_x), int(_y)); 
	return returnedValue;
}


float LivingSpot::getX(){return _x;}
float LivingSpot::getY(){return _y;}


int LivingSpot::computeInteraction(LivingSpot* spotArray, int numSpots) {
 // rem: the total number of spots is: numberOfSpots, but some may not be active (alive mode)
 // the currect spot ("this") has id: identifier (a number from 0 to numberOfSpots)
collision=false; //initialize collision flag (rem: collision will take place when in REPULSIVE distance)
if (alive==true) {
		for (int i=identifier+1; i<numSpots; i++) {
			if (spotArray[i].alive==true) { // else, do nothing (just skip this spot)
				float ux, uy, sqinterSpotDist, interSpotDist;
				ux=spotArray[i]._x-_x;
				uy=spotArray[i]._y-_y;
				interSpotDist=sqrt(ux*ux+uy*uy)+6; // to avoid singularities
				
                                if (interSpotDist<rangeInteractionLong) { //otherwise do nothing
                                  // REM: force here can be on 1/r^2, 1/r or proportional to the distance...
                                  // (a) in 1/r^2:
                                  float interForce=spotArray[identifier].charge*spotArray[i].charge/(interSpotDist*interSpotDist*interSpotDist); // *interForceFactor is done at the update function
                                   // (b) in 1/r:
                                  //float interForce=spotArray[identifier].charge*spotArray[i].charge/(interSpotDist*interSpotDist); // *interForceFactor is done at the update function
                                   // (c)  proportional to r (spring like):
                                  //float interForce=spotArray[identifier].charge*spotArray[i].charge*(interSpotDist-rangeInteractionShort); // *interForceFactor is done at the update function
                                  
                                  if (interSpotDist<rangeInteractionShort) {  // this means repulsive force, and "collision"
                                    collision=true;
                                    // (a) in 1/r^2:
                                    interForce=2.0/(rangeInteractionShort*rangeInteractionShort*rangeInteractionShort)-interForce;
                                     // (b) in 1/r:
                                    //interForce=2.0/(rangeInteractionShort*rangeInteractionShort)-interForce;
                                     // (c) proportional to r (spring like): nothing needs to be done
                                   // ...
                           
                                  } else { // this means attractive force, but not collision
                                   // interForce*=1.0; // nothing is needed
                                  }
                                  // now, compute the X and Y components and add to the forces on THIS particle, and on the other particle:
                                  interForceX+=interForce*ux;
                                  interForceY+=interForce*uy;
                                  
                                  spotArray[i].interForceX-=interForce*ux;
                                  spotArray[i].interForceY-=interForce*uy;
                                } 
			
			}
	 }
 }
 return(collision);
}

// Update spot position and kinematic parameters using Newton dynamics:
int  LivingSpot::newtonDynamics(int numSpots){ // rem: we could have dt as a time increment parameter, but we would need a precision of microseconds. I leave it for now...
	
	totalForceX=0; totalForceY=0;

      // compute dt in the loop:
       if (micros()>oldTime)  dt=0.001*(micros()-oldTime); // dt in MILLI-SECONDS!
      oldTime=micros();
      //Serial.println(dt); // it turns out that dt is about 5 ms
      dt*=slowTime;
      
      // We don't need the speed for integrating using the VERLET algorithm, but we may need it for sending data for sound, or if we compute the viscosity force
      // non explicitly (meaning, not integrating it on the verlet formula):
	vx=(_x-x1)/dt;
	vy=(_y-y1)/dt;
      
      float distCenter=sqrt((_x- CENTER_AD_MIRROR_X)*(_x- CENTER_AD_MIRROR_X)+(_y- CENTER_AD_MIRROR_Y)*(_y- CENTER_AD_MIRROR_Y))+10; // 0.1 to avoid singularities
      
        if (numSpots>1) computeNormalVector(_x, _y, _saccadeRadius);
        boolean touch=computeNormalVector(_x, _y, _saccadeRadius);

	//(1) bouncing on contours (and mirror limits?):
	if (bouncingForceMode==true) {
		if (touch) { // if something was in the way of the spot, 
		// compute the bouncing force as a function of the recentering vector:
                // (nx, ny) (normed), or (dx, dy), whose norm is NormVector,
               //  everything weighed by the boucingForceFactor:
               
               /*
               // TEST: force equal to repulsive force in case of interparticle interaction, with the norm of the recentering 
               // vector representing the proximity of the "two particles":
               float daux=NormVector/_saccadeRadius*rangeInteractionShort/1.414; 
               daux=interForceFactor*(2.0/(rangeInteractionShort*rangeInteractionShort)-1.0/(daux*daux));
                // Note: the meaning of this, is that when NormVector=_saccadeRadius (meaning half of the saccade is inside a black zone), then the
                // repulsive force felt correspond to that of two particles of charge=1 in the exact location when their interaction force is 0 - i.e., their 
                // distance is rangeInteractionShort/sqrt(2). NormVector is always < _saccadeRadius, so the force is always "repulsive" (pointing towards the 
                // white zone). The bouncingForceFactor is an additional parameter.
			totalForceX=-nx*daux*bouncingForceFactor;
			totalForceY=-ny*daux*bouncingForceFactor;
                */
                totalForceX=-nx*bouncingForceFactor;
		totalForceY=-ny*bouncingForceFactor;

		}
	}
	
  // (1.b) bouncing on countours by inverting speed:
      if (bouncingSpecularMode) {
        if(touch){ // if something was in the way of the spot
                
                // x1 is the current position, _x is the past position. We need to reverse them:
                float auxX=_x; _x=x1; x1=auxX;
                float auxY=_y; _y=y1; y1=auxY;
                
		}
      }


	// (2) particle interaction force:
    if (particleInteractionMode==true) {
		totalForceX+=interForceX*interForceFactor;
		totalForceY+=interForceY*interForceFactor; // the force produced by interaction with other particles
    }
	
	// (3) Central force (SPRING!!):
    if (centralForceMode==true) {
                totalForceX-=centralForceFactor*(_x- CENTER_AD_MIRROR_X); 
		totalForceY-=centralForceFactor*(_y- CENTER_AD_MIRROR_Y); 
    }
	
	// (4) parallel force:
	if (parallelForceMode==true) {
		totalForceX+=gx*mass;
		totalForceY+=gy*mass; 
	}
	
	//(5) viscosity force (for verlet integration this could be done in the integration formula...):
	if (visForceMode==true) {
        // REM: if visForceFactor is the fraction of the speed lost to friction (no unit) used in the verlet formula, then the 
        // factor that we need to use here is: visForceFactor*mass/dt (unit is kg/sec):
		totalForceX-=visForceFactor*vx*mass/dt;
		totalForceY-=visForceFactor*vy*mass/dt;	
	}
	
    // Compute the acceleration at time t from the interaction forces (at time t): 
    ax=totalForceX/mass;
    ay=totalForceY/mass;

    // INTEGRATioN USING VERLET ALGORITHM: 
    
    //First, compute the new position using the two previous positions, plus the current acceleration (computed from the CURRENT position): 
    // NOTE: definitions are as follows: x2=x(t+dt), _x=x(t) and x1=x(t-dt)
        // With viscosity in the formula:
        // REM: visForceFactor is the fraction of the speed lost to friction (no unit!)
        //float x2=(2-auxVerletVisc)*_x-(1-auxVerletVisc)*x1+ax*dt*dt; 
	//float y2=(2-auxVerletVisc)*_y-(1-auxVerletVisc)*y1+ay*dt*dt;

        float x2=2*_x-x1+ax*dt*dt;
	float y2=2*_y-y1+ay*dt*dt;

        // save the position at time t for the new iteration as the "old" position t-dt:
        x1=_x; y1=_y;
        
        // update the new position:
	updatePosition(x2, y2); // ATTN: the update of the new position is not done directly, because it will be checked for consistency with the mirrors excursion limits (moveSpot calls updatePosition first)
        // now, _x and _y have the "new" position.
        
        // reset interforce for THIS particle:
        interForceX=0; interForceY=0;
}

// Refraction following Snell Law:
int  LivingSpot::lightRefraction(){ 
  static int countsign=1;
  static float n1; // this way the variable does not need to be declared global
  float alpha1, alpha2;
  float v2x, v2y=0;
  float v1x, v1y;
  float vnewx, vnewy;
  float rotx, roty;
  float normSpeed=sqrt(vx*vx+vy*vy); // this is conserved, but normally it should change with the index!!!
		

 // the following line is just to slow down in case we use processing to check the output of the mirrors: 
   #ifdef _NOHARD
    computeNormalVector(_x, _y, _saccadeRadius);
   #endif

	//_averageLockIn = _avLockInTab[0];
	//for(int i=1; i<_nbAvr; i++) {_averageLockIn += _avLockInTab[i];}
	//_averageLockIn /= _nbAvr;
	
        #ifdef _NOHARD
        int currLockIn =test_noHardware_PD(_x, _y);
	#else
        float currLockIn = 0;
        //moveSpot(_x, _y);
        for (int i=0; i<2; i++) currLockIn+=IO.FastAnalogReadPin8();//IO.LockInRead_Volts();
        currLockIn/=2.0;
        #endif

	float n2=refractionIndex(int(currLockIn)); // this is the calculation of the index of refraction from the intensity
	if (firstTimeStartRefraction) {
            n1=n2;            
            firstTimeStartRefraction=false;
            //placeY=placeY+countsign*25; if (placeY>2450) countsign*=-1; if (placeY<550) countsign*=-1; //placeX=550; };
            placeY=placeY+countsign*50; if (placeY>2000+200) countsign*=-1; if (placeY<2000-200) countsign*=-1; //placeX=550; };
        }
	if (abs(n1-n2)>.1) {
		
              // this is for activating the green laser at the transition by REDOING a green saccade:
                collision=true; 
                
		//goWhite();
		//spot.moveSpot(int(spot.getX() - spot.vx), int(spot.getY() - spot.vy));
		
                _x=_x-vx;
                _y=_y-vy;
                IO.writeOutX(int(_x));
	        IO.writeOutY(int(_y));
                //delayMicroseconds(100);
                //moveSpot(_x-0.5*vx, _y-0.5*vy);
                
                // this makes the measuring saccade in green (no need to re-do the saccade, but perhaps less acurate measurement because of interference):
                 IO.setGreenPower(LOW);
		if(computeNormalVector(_x, _y, _saccadeRadius)){ 
				
                        v1x=(nx * vx + ny * vy); // rem: norm of (nx, ny) is 1
                        if (v1x<0) {// this means that n and v point to different directions with respect to the interface
                        nx=-nx; ny=-ny; 
                        v1x=-v1x;  
                        }
                        
                        // compute +90 deg rotated normal vector forming orthonormal base with (nx, ny):
                        rotx=-ny; 
                        roty=nx;
                        v1y=(rotx * vx + roty * vy);
			
                        // compute incident angle alpha1:
                        alpha1=asin(v1y/normSpeed);
                        
                        // Two cases: TIR or normal refraction:
                        if (abs(sin(alpha1))<(1.0*n2/n1)) { // this means normal refraction
                        alpha2=asin(1.0*n1/n2*sin(alpha1));
                        v2x=cos(alpha2)*normSpeed;
                        v2y=sin(alpha2)*normSpeed;
                        } else { // this means total internal reflexion (TIR):
                        v2x=-v1x;
                        v2y=v1y;
                        }
			
                        // finally, reconstruct vector v2 in mirror X and Y coordinates:
			vnewx=v2x*nx + rotx*v2y;// * rotx;
                        vnewy=v2x*ny + roty*v2y;// * roty;
                        moveSpot(_x+vnewx, _y+vnewy); 
		      //moveSpot(_x -vx+vnewx, _y - vy+vnewy); // the factor on the speed is for going quickly on the other side of the interface
                        vx=vnewx; vy=vnewy;
}
	} else { // this means that indexes are too closr (their difference may be just a matter of noise)
		moveSpot(_x+ vx, _y+ vy); // rectilinear motion (rem: could be bent by "gravitanional forces" using newtonian dynamics...)
                //if(_idAv == _nbAvr) _idAv = 0;
                //_avLockInTab[_idAv] = currLockIn;
                //_idAv++;	
                
                 collision=false; 
        }
        
         IO.setGreenPower(HIGH);
         
        // save "old" value of index:
        n1=n2;
        //delay(10);
}

float  LivingSpot::refractionIndex(int intensity) {
  //Serial.println(intensity);
  //Serial.println(fixedThreshold);
	//if (intensity>(MAX_INTENSITE- MIN_INTENSITE)*.5+MIN_INTENSITE) return(1); else return(2);
      //  if (intensity>(MAX_INTENSITE- MIN_INTENSITE)*.5+MIN_INTENSITE) return(1); else return(2);
     
     // for white background:
     //  if (intensity>fixedThreshold) return(1.7); else return(1);
       // for black background:
        if (intensity>fixedThreshold) return(1); else return(1.7);
        
        
//        if (intensity<fixedThreshold) return(1); 
//        else if (intensity<fixedThreshold2) return(1.6);
//        else return(100);
	
// return(1.0*intensity/750+1);
}


//draw a line until it enter in a dark area, compute the reflexion vector using the normal vector of the contour
//return 1 if bounce on a dark area, return 0 if it go straight
int LivingSpot::fastBouncing(){ 
  
  // the following line is just to slow down in case we use processing to check the output of the mirrors: 
   #ifdef _NOHARD
  computeNormalVector(_x, _y, _saccadeRadius);
   #endif
  
  _averageLockIn = _avLockInTab[0];
  for(int i=1; i<_nbAvr; i++) {_averageLockIn += _avLockInTab[i];}
  _averageLockIn /= _nbAvr;
  

        #ifdef _NOHARD
        int currLockIn =test_noHardware_PD(_x, _y);
	#else
        int currLockIn = IO.FastAnalogReadPin0();
        #endif
  
  if (currLockIn < _averageLockIn / thresholdFactorFB) {
    
	//TO TUNE:
	//go back before performing a saccade (depend of the speed)
    moveSpot(_x - vx, _y - vy);
	//delayMicroseconds(100);
   
    //spot.computeNormalVector();
    if(computeNormalVector(_x, _y, _saccadeRadius)){
      
      float norma=(nx * vx + ny * vy);
      
      //if(norma<0.1) spot.updatePosition(int(spot.getX() + spot.dx), int(spot.getY() + spot.dy));
      
      vx = -2.0 * norma * nx + vx;
      vy = -2.0 * norma * ny + vy;
      
      moveSpot(_x + vx + dx, _y + vy + dy);
   	  
	  return 1;
	  
    }
	//else ??
	return 1;
    
  } 
  else {
    moveSpot(_x + vx, _y + vy);
    if(_idAv == _nbAvr) _idAv = 0;
    _avLockInTab[_idAv] = currLockIn;
    _idAv++;
    return 0; 
  }
}


//scan and bounce on dark edge
int LivingSpot::scanBouncing(){ 
  
  _averageLockIn = _avLockInTab[0];
  for(int i=1; i<_nbAvr; i++) {_averageLockIn += _avLockInTab[i];}
  _averageLockIn /= _nbAvr;
  
   #ifdef _NOHARD
        int currLockIn =test_noHardware_PD(_x, _y);
	#else
        int currLockIn = IO.FastAnalogReadPin0();
        #endif
  
  if (currLockIn < _averageLockIn / thresholdFactorFB) {
	vx *= -1.0;
	moveSpot(_x + vx, _y + vy);
	return 1;
    
  } 
  else {
    moveSpot(_x + vx, _y);
     
    if(_idAv == _nbAvr) _idAv = 0;
    _avLockInTab[_idAv] = currLockIn;
    _idAv++;
    return 0; 
  }
}


//normal bouncing, make a saccade even if nothing
//return 1 if bounce on a dark area, return 0 if it go straight
int LivingSpot::simpleBouncing(int numSpots){ 
    if (numSpots>1) computeNormalVector(_x, _y, _saccadeRadius);
    if(computeNormalVector(_x, _y, _saccadeRadius)){
		
		float norma=(nx * vx + ny * vy);
		
		vx = -2.0 * norma * nx + vx;
		vy = -2.0 * norma * ny + vy;

                // New: damping as it collides
                vx*=bounce_collisionDamping;
                vy*=bounce_collisionDamping;

   // limit the speed: 
    float normSpeed=sqrt(vx*vx+vy*vy); 
    if (normSpeed>phoenixNormSpeed) {
    vx*=phoenixNormSpeed/normSpeed;
    vy*=phoenixNormSpeed/normSpeed;
    }		

		updatePosition(_x + dx + vx, _y + dy + vy);		
		return 1;
		
	} 
	else {
  
                // New: damping as it moves
                vx*=bounce_movingDamping;
                vy*=bounce_movingDamping;
                
                   // limit the speed: 
    float normSpeed=sqrt(vx*vx+vy*vy); 
    if (normSpeed>phoenixNormSpeed) {
    vx*=phoenixNormSpeed/normSpeed;
    vy*=phoenixNormSpeed/normSpeed;
    }
		updatePosition(_x + vx, _y + vy);
		return 0; 
	}
  
}



//follow a contour; compute the correction vector to stay on a contour
//return true if capable to follow, return false if the contour is lost
bool LivingSpot::moveAlongContour(int numSpots){
  bool returnValue=false;
        //move to the first position of the saccade:
	//IO.writeOutX(int(_x + _saccadePosTab[0]));
	//IO.writeOutY(int(_y + _saccadePosTab[1]));
	//delayMicroseconds(1000);
  
  //perform a saccade:
  if (numSpots>1) computeNormalVector(_x, _y, _saccadeRadius);
  //computeNormalVector(_x, _y, _saccadeRadius);
  if(computeNormalVector(_x, _y, _saccadeRadius)){ // this means that the saccade is on something interesting
    returnValue=true;
    
    float normAuxVector = speedContourFollowing * _saccadeRadius;
 
    //Then, compute the 90CCW vector with respect to (dx,dy), with norm equal to normAuxVector : 
    float px= -1.0 * ny * normAuxVector;
    float py= 1.0 * nx * normAuxVector;
   
    // Second, adjust the correcting vector so as to mantain penetration on the countour: 
    // this part takes the correction vector (dx, dy) computed in angular correction, and readjust it so as to maintain a "fixed penetration" on the 
    // lighted zone. The fixed penetration length is equal to fixedPenetration=penetrationFactor*saccadeDiam;
    float fixedPenetration = 1.0 * _saccadeRadius;
    float correctingFactor = 0.5 * (NormVector - fixedPenetration); // can be negative or positive. By putting <1, we "converge" smoothly to the right position
    
    int dxx = nx * correctingFactor + px;
    int dyy = ny * correctingFactor + py;
   
    // Finally, update position of center of saccade by adding pVector and dVector togheter: 
    updatePosition(_x + dxx, _y + dyy); 
  }
  
  return(returnValue);
} 

bool LivingSpot::searchContour(int numSpots) {
  // First, search for something with a radius proportional to the search cycle (searchCounter):
  if(numSpots>1) computeNormalVector(_x, _y, _saccadeRadius*(searchCounter*searchCounter/10+0.5));
  if(onContour=computeNormalVector(_x, _y, _saccadeRadius*(searchCounter*searchCounter/10+0.5))) { // this means that when searchCounter=0, radius is _saccadeRAdius, and when searchCounter=100, radius is 10*_saccadeRadius
    if (whiteProportion<0.45) 
        updatePosition(_x +_saccadeRadius*(searchCounter*1.0/11+1)*nx/2, _y + ny*_saccadeRadius*(searchCounter*searchCounter/10+0.5)/2); 
    else 
        updatePosition(_x -_saccadeRadius*(searchCounter*1.0/11+1)*nx/2, _y - ny*_saccadeRadius*(searchCounter*searchCounter/10+0.5)/2); 
    searchCounter=0;
    
    return(true);  
} 
return(false);
}


//draw a squarre corresponding to the maximum area.
//nloops is the number of time it will draw the square.
void LivingSpot::drawMaxArea(int nloops){
	int i, idloop;	
	
	for(idloop=0; idloop < nloops; idloop++){  
		for(i=yMin; i<yMax; i+=15){
			IO.writeOutX(xMin);
			IO.writeOutY(i);
		}
		IO.writeOutX(xMin);
		IO.writeOutY(yMax);
		delayMicroseconds(250);
		for(i=xMin; i<xMax; i+=15){
			IO.writeOutX(i);
			IO.writeOutY(yMax);
		}
		IO.writeOutX(xMax);
		IO.writeOutY(yMax);
		delayMicroseconds(250);
		for(i=yMax; i>=yMin; i-=15){
			IO.writeOutX(xMax);
			IO.writeOutY(i);
		}
		IO.writeOutX(xMax);
		IO.writeOutY(yMin);
		delayMicroseconds(250);
		for(i=xMax; i>=xMin; i-=15){
			IO.writeOutX(i);
			IO.writeOutY(yMin);
		}
		IO.writeOutX(xMin);
		IO.writeOutY(yMin);
		delayMicroseconds(250);
	}
} 


//--------draw Logo--------
//take care of the maximum boundaries

//draw the letters SLP horizontaly
//possibility to change the position and the size on X and y axis
//nloops is the number of time it will draw the logo
void LivingSpot::drawSLPLogo(int centerLogo_X, int centerLogo_Y, int sizeLogo_X, int sizeLogo_Y, int nloops){
	for(int n=0; n<nloops; n++){
		for(int i=tableSLPLogo_size-1; i>=0; i--){
			moveSpot( centerLogo_X + tableSLPLogo_X[i] * sizeLogo_X, centerLogo_Y + tableSLPLogo_Y[i] * sizeLogo_Y);
			delayMicroseconds(84);
		}
		for(int i=0; i<tableSLPLogo_size; i++){
			moveSpot( centerLogo_X + tableSLPLogo_X[i] * sizeLogo_X, centerLogo_Y + tableSLPLogo_Y[i] * sizeLogo_Y);
			delayMicroseconds(84);
		}
	}
}

//draw the letters SLP oriented with an angle in deg
//possibility to change the position and the size on X and y axis
//nloops is the number of time it will draw the logo
void LivingSpot::drawSLPLogoAngle(float angleLogo, int centerLogo_X, int centerLogo_Y, int sizeLogo_X, int sizeLogo_Y, int nloops){
  for(int n=0; n<nloops; n++){
    float ang = angleLogo * 0.01745;
    float cosAngX = cos(ang) * sizeLogo_X;
    float sinAngX = sin(ang) * sizeLogo_X;
    float cosAngY = cos(ang) * sizeLogo_Y;
    float sinAngY = sin(ang) * sizeLogo_Y;
    for(int i=tableSLPLogo_size-1; i>=0; i--){
      moveSpot( centerLogo_X + tableSLPLogo_X[i] * cosAngX + tableSLPLogo_Y[i] * sinAngY , centerLogo_Y - tableSLPLogo_X[i] * sinAngX + tableSLPLogo_Y[i] * cosAngY);
    }
    for(int i=0; i<tableSLPLogo_size; i++){
      moveSpot( centerLogo_X + tableSLPLogo_X[i] * cosAngX + tableSLPLogo_Y[i] * sinAngY , centerLogo_Y - tableSLPLogo_X[i] * sinAngX + tableSLPLogo_Y[i] * cosAngY);
    }
  }
}

//draw the letters SLP and make it doing a complete 360 deg loop
//possibility to change the position, the size on X and Y axis and the speed of rotation
void LivingSpot::drawSLPLogoRotating(int speedRotation, int centerLogo_X, int centerLogo_Y, int sizeLogo_X, int sizeLogo_Y){
	for(int a = 0; a<=360; a+=speedRotation){
		drawSLPLogoAngle(a, centerLogo_X, centerLogo_Y, sizeLogo_X, sizeLogo_Y, 1);
	}
}




//---------------Methods related to the saccade---------------

int LivingSpot::setRadiusSaccade(int radius) {  // will change _saccadeRadius, 
	int returnvalue=-1;
	if ((radius>0)&&(radius<MAXRADIUS)) {
		_saccadeRadius=radius;
		returnvalue=0;
                // rem: no need to recalculate the saccade table! this must be done only when changing the number of samples
		} // else do nothing
    return(returnvalue);
}

//creates and update the tables related to the saccade when we change the number of samples (not the radius)
void LivingSpot::setNumSamples(int numSamples){

	//if the tables already exist, delete them
	//if(_saccadePosTab) free(_saccadePosTab);
	//if(_saccadeIntesityTab) free(_saccadeIntesityTab);
	//if(_saccadeBinaryTab) free(_saccadeBinaryTab);
	//if(_transBinaryTab) free(_transBinaryTab);
	
	//update the number of samples
	_numSamples = numSamples;
	
	//create new table with the right size
	//_saccadePosTab = (float*)malloc(sizeof(float) * _numSamples * 2);
	//_saccadeIntesityTab = (int*)malloc(sizeof(int) * _numSamples);
	//_saccadeBinaryTab = (byte*)malloc(sizeof(byte) * _numSamples);
	//_transBinaryTab = (byte*)malloc(sizeof(byte) * _numSamples);
	
	//compute the position of each sampling spot of the saccade
	float delta_angle = 2.0 * PI / _numSamples;
	for(int i=0; i<_numSamples; i++){
		_saccadePosTab[2 * i] = cos(delta_angle * i);
		_saccadePosTab[2 * i + 1] = sin(delta_angle * i);
	}
}


//performs a scanning saccade centered on the current x and y,
//mesure the lockIn amplifier value for each sampling spot,
//pre-process the saccade data,
//compute normal vector
int LivingSpot::_saccade(float x, float y, int radius){  
	int i;
        int counterwhite=0; // computation of white proportion:
	
        // FIRST, ACQUIRE DATA and compute statistics:
	float averageI = 0.0;
		// note: maxI and minI are defined as public variables now (easy access as relevant spot information). 
		maxI = 0;
		minI = 1023;// the 8 bit readout (fast reading) gives values between 0 and 255, and in high res (10 bits), values are from 0 to 1023
		
		float sigmI;
		float d;
		
                register unsigned int auxI;
		for(i=0; i<_numSamples; i++){
			//move the spot to each position of the saccade
			IO.writeOutX(int(1.0*_x + _saccadePosTab[2 * i]*radius));
			IO.writeOutY(int(1.0*_y + _saccadePosTab[2 * i + 1]*radius));
			
			//delayMicroseconds(100); //must calibrate the timing
			delayMicroseconds(80); 
  
			//read the value of the lockin amplifier at each position
                        #ifdef _NOHARD
                        // For test without hardware: 
                        auxI =test_noHardware_PD(int(1.0*_x + _saccadePosTab[2 * i]*radius), int(1.0*_y + _saccadePosTab[2 * i + 1]*radius));
                        #else
                        auxI = IO.FastAnalogReadPin8(); //analogRead(8); 
                       #endif

			//directly compute maximum, minimum and first order moment (average)
			if(auxI > maxI) maxI = auxI;
			if(auxI < minI) minI = auxI;
			averageI += auxI;

                        _saccadeIntesityTab[i]=auxI;
		}
		averageI /= 1.0 * _numSamples; // rem: averageI is a floating point variable.
		
		// Second order moment (variance):
                /*
		sigmI=0; // REM: averageI and sigmauxI are floating point variables, not integers!
		for(i=0; i<_numSamples; i++){ 
			d = 1.0 * (_saccadeIntesityTab[i] - averageI);
			sigmI += d * d;
		}
		sigmI = sqrt(sigmI / (_numSamples-1)); // unbiased sample estimator of the variance
		*/

	//----------auto threshold-----------
	if(thresholdingMode) {

		//float threshold_factor=.6;//2.0/3; // it is a #define since 7.4.2010
		float MIN_CONTRAST_AD;// = 50;// computed in rangeCalibration or manually set by reading the data from the photodetector...
		MIN_CONTRAST_AD=(MAX_INTENSITE-MIN_INTENSITE)/contrast_factor;//0.6;//(130-80)/.6;

                // First, check that we have both a white and a black part on the saccade (otherwise do nothing):
                //if (maxI<130) return(0);
                //if (minI>80) return(0);
              // MIN_CONTRAST_AD=(MAX_INTENSITE-MIN_INTENSITE)/contrast_factor;//0.6;//(130-80)/.6;
                
                // Then, compute the automatic threshold:
		// Minimum intensity variation MIN_CONTRAST_AD needed in order to set the automatic threshold:
		if ((maxI-minI) > MIN_CONTRAST_AD ) { // MIN_CONTRAST_AD is calculated in RangeCalibration (it is either manual or equal to 2xsigmauxI for instance). 
			autoThreshold = 1.0 * (maxI-minI) * threshold_factor + minI;	// threshold_factor = 2/3 or 1/2 is a good value.
		}
		else {// ... otherwise, we consider that the saccade is FULL on something white
			return 0; //not possible to compute a normal vector
		}
		
		
		// Binarization of saccade data using autothreshold:
		for(i=0; i<_numSamples; i++){
			if(_saccadeIntesityTab[i] >= autoThreshold){
				_saccadeBinaryTab[i] = 1;
                                counterwhite++;
			}
			else{
				_saccadeBinaryTab[i] = 0;
			}
		}
		
              // HERE: ERODE, DILATE...

       
		
	}
	//----------fixed threshold----------
	else {
		
             // Binarization of saccade data with fixed threshold:
		for(i=0; i<_numSamples; i++){
			if(_saccadeIntesityTab[i] >= fixedThreshold){
				_saccadeBinaryTab[i] = 1;
                                 counterwhite++;
			}
			else{
				_saccadeBinaryTab[i] = 0;
			}
		}
		
		
		
	}

       
       // Compute white proportion:
       whiteProportion=1.0*counterwhite/_numSamples;	
            	
       //if the saccade is totaly light or totaly dark, not possible to compute a normal vector
       if(counterwhite==0 || counterwhite==(_numSamples-1)) return 0; else return 1;
}


// This function computes the correction vector dy and dx (and its normalized value nx, ny)
// REM: This is not completely meaninful in physical terms, because it should be more like the 
// correction ANGLE depending on the distance of the target, but may be enough for what we want to do here (countour tracking). 
// REM: return 1 if the computation of the vector succeded, otherwise return 0
int LivingSpot::computeNormalVector(float x, float y, int radius) {
	
	// First, perform a scaning saccade:
	if(_saccade(x, y, radius)){
		
		//initialize normal vector:
		dx=0; dy=0;
		
		int i; // counter
		
		int for_averaging=0; // number of samples for averaging the target_angle
		int zone_flag=0; // flag to help counting the number of lighted/dark regions
		int light_counter=0, dark_counter=0; // region counters.
		int min_angle=0, max_angle=_numSamples-1; // default value of the angles (in samples) of the considered region
		target_angle=0; // direction of displacement
		float delta_angle=0; // values of the angular extension of the considered region
		bool flag_min_angle=true; // flag for min_angle and max_angle calculation
		
		// float NormVectorMultiplier=1; // Multiplier of NormVector [a public variable now]
		// float NormVector; // Norme du vecteur de deplacement ANGULAIRE! [a public variable now]
		
		
		if (binary_smoothing_Mode) {// smooth data before computing the number of "light-zones"		
			_Dilate(); // since 28.2.2005, Erode and Dilate does not take arguments, because they refer to the global variable _saccadeBinaryTab
			_Erode();
			_Erode();
			_Dilate();
		}
		
		
		// 5): compute the number of lighted/dark regions.
		// REM: the number of items on _saccadeBinaryTab array are just equal to the number of points (numSamples)
		zone_flag=_saccadeBinaryTab[0];
		for (i=0; i<_numSamples; i++) {
			if (zone_flag == _saccadeBinaryTab[i]) {
				if (zone_flag == 1)
					light_counter++;
				else
					dark_counter++;
				zone_flag = 1 - zone_flag;
			}
		}
		
		
		// 6) calculation of the "target" angle depending on the number of lighted/dark regions
		// 0 -  NO lighted region (target is lost), nothing is done
		// 1 - one and only one lighted region, average angle is calculated on the lighted zone
		// Nothing is done if only lighted region (no dark region). Two dark regions possible.
		// 2.1 - two lighted regions and one and only one dark region:
		// average angle is calculated on the dark zone, and it should be added PI radians.
		// 2.2 - two lighted regions and two dark regions:
		// average angle is calculated on the second lighted zone.
		// 3 (default) - Other cases : nothing is done yet.	
				
		switch (light_counter) 
		{
			case 0: // nothing is done.
				return 0;
				
			case 1: // unique lighted region
				i=0;
				while(_saccadeBinaryTab[i] == 0) i++; // skip the dark zone
				while((i<_numSamples) && (_saccadeBinaryTab[i]==1)){ // we are on the unique ligthed zone
					target_angle = target_angle + i;
					for_averaging++;
					if(flag_min_angle){	
						flag_min_angle = false; 
						min_angle = i;
					}
					i++;
				}
				max_angle=i-1;// max_angle contains the index of the last illuminated sample
				
				//if (for_averaging!=numSamples) { //otherwise it means no correction is needed: the saccade is all inside the light zone
				if((max_angle-min_angle) != (_numSamples - 1)){ //otherwise it means no correction is needed: the saccade is all inside the light zone
					delta_angle = 2.0 * PI / _numSamples * (max_angle-min_angle); // angular extension INSIDE the lighted zone
					NormVector = _saccadeRadius * (1 + cos(delta_angle/2)); // norm of the angular repositioning vector
					//target_angle=1.0*(max_angle-min_angle)*PI/numSamples;
					target_angle = target_angle / for_averaging * 2.0 * PI / _numSamples; // direction angle	
					//target_angle+=2*PI/numSamples*mirror_delay_Samples+teta0;
					// The corrective angles are therefore:
					nx = cos(target_angle + error_angle); 
					ny = sin(target_angle + error_angle);
					dx = NormVector * nx;
					dy = NormVector * ny;
					
					return 1;
				}
				return 0;
				
			case 2: // two lighted regions
				if(dark_counter == 1){ // one and only one dark region
					i=0;
					while(_saccadeBinaryTab[i] == 1) i++; // skip first lighted zone
					while((i<_numSamples) && (_saccadeBinaryTab[i]==0)){ // we are on the unique dark zone
						target_angle = target_angle + i;
						for_averaging++;
						if (flag_min_angle) {	
							flag_min_angle = false; 
							min_angle = i;
						}
						i++;
					}
					max_angle = i-1;// max_angle contains the index of the last dark sample
					
					delta_angle = 2.0 * PI / _numSamples * (max_angle-min_angle); // angular extension
					NormVector = _saccadeRadius * (1 - cos(delta_angle/2)); // new repositioning vector:
					// addition of PI radians is necessary when estimating the target_angle in the dark zone:
					//target_angle=1.0*(max_angle-min_angle)*PI/numSamples+PI;
					target_angle = target_angle / for_averaging * 2.0 * PI / _numSamples; // direction angle	
					//target_angle+=2*PI/numSamples*mirror_delay_Samples+teta0;
					// the corrective angles:
					nx = -cos(target_angle + error_angle); 
					ny = -sin(target_angle + error_angle);
					dx = NormVector * nx;
					dy = NormVector * ny;
					
					return 1;
				}
				else{
					//not done yet
					return 0;
				}
				
			default: // more than two lighted regions: nothing is done
				return 0;
		}
		
	}
	
	else return 0;
}


// MORPHOLOGIC filters for smoothing binary data:
void LivingSpot::_Dilate() // Dilation
{
	// rem: it is assumed that the size of _saccadeBinaryTab array is _numSamples (global variable)
	int i; //counter
	
	_transBinaryTab[0] = _saccadeBinaryTab[_numSamples-1] | _saccadeBinaryTab[0] | _saccadeBinaryTab[1];
	for (i=1; i<_numSamples-1;i++) {
		_transBinaryTab[i] = _saccadeBinaryTab[i-1] | _saccadeBinaryTab[i] | _saccadeBinaryTab[i+1];
	}
	_transBinaryTab[_numSamples-1] = _saccadeBinaryTab[_numSamples-2] | _saccadeBinaryTab[_numSamples-1] | _saccadeBinaryTab[0];

	for (i=0; i<_numSamples;i++) _saccadeBinaryTab[i] = _transBinaryTab[i];
}

void LivingSpot::_Erode() // Erosion
{
	// rem: it is assumed that the size of _saccadeBinaryTab array is numSamples (global variable)
	int i; //counter
	
	_transBinaryTab[0] = _saccadeBinaryTab[_numSamples-1] & _saccadeBinaryTab[0] & _saccadeBinaryTab[1];
	for (i=1; i<_numSamples-1;i++) {
		_transBinaryTab[i] = _saccadeBinaryTab[i-1] & _saccadeBinaryTab[i] & _saccadeBinaryTab[i+1];
	}
	_transBinaryTab[_numSamples-1] = _saccadeBinaryTab[_numSamples-2] & _saccadeBinaryTab[_numSamples-1] & _saccadeBinaryTab[0];

	for (i=0; i<_numSamples;i++) _saccadeBinaryTab[i] = _transBinaryTab[i];
}
















